#ifndef CODEQR_HPP
#define CODEQR_HPP
#include <opencv2/opencv.hpp>
#include <vector>
#include <opencv2/aruco.hpp>

class CodeQR
{
public:
    std::string data;
    std::vector<cv::Point> vertex;
    float markerLength;
public:
    CodeQR()
        : markerLength(13)
    {

    }

    void operator =(const CodeQR &src)
    {
        data=src.data;
        markerLength=src.markerLength;
        vertex.assign(src.vertex.begin(),src.vertex.end());
    }

    CodeQR(const CodeQR &src)
    {
        operator =(src);
    }

    cv::Point2f Center()
    {
        if(vertex.size()<4)
            return cv::Point2f();
        cv::Vec3f diagonal0=cv::Vec3f(vertex[0].x, vertex[0].y, 1).cross(cv::Vec3f(vertex[2].x, vertex[2].y, 1));
        cv::Vec3f diagonal1=cv::Vec3f(vertex[1].x, vertex[1].y, 1).cross(cv::Vec3f(vertex[3].x, vertex[3].y, 1));
        cv::Vec3f center3=diagonal0.cross(diagonal1);
        return cv::Point2f(center3[0]/center3[2], center3[1]/center3[2]);
    }

    cv::Vec6d Center(const cv::Mat &camMatrix, const cv::Mat &distCoeffs)
    {
        if(vertex.size()<4 || camMatrix.empty() || distCoeffs.empty())
            return cv::Vec6d();

        std::vector< std::vector< cv::Point2f > > corners(1, std::vector< cv::Point2f >());
        std::vector< cv::Vec3d > rvecs;
        std::vector< cv::Vec3d > tvecs;

        corners[0].push_back( cv::Point2f(vertex[0].x, vertex[0].y) );
        corners[0].push_back( cv::Point2f(vertex[1].x, vertex[1].y) );
        corners[0].push_back( cv::Point2f(vertex[2].x, vertex[2].y) );
        corners[0].push_back( cv::Point2f(vertex[3].x, vertex[3].y) );

        cv::aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs, tvecs);

        return cv::Vec6d(rvecs[0][0], rvecs[0][1], rvecs[0][2], tvecs[0][0], tvecs[0][1], tvecs[0][2]);
    }



};





#endif // CODEQR_HPP
